/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/canbus/vehicle/agribot/protocol/accel_cmd_1e3.h"

#include "modules/drivers/canbus/common/byte.h"

namespace apollo {
namespace canbus {
namespace agribot {

using ::apollo::drivers::canbus::Byte;

const int32_t Accelcmd1e3::ID = 0x1E3;

// public
Accelcmd1e3::Accelcmd1e3() { Reset(); }

uint32_t Accelcmd1e3::GetPeriod() const {
  // TODO(QiL) :modify every protocol's period manually
  static const uint32_t PERIOD = 50 * 1000;
  return PERIOD;
}

void Accelcmd1e3::UpdateData(uint8_t* data) {
  set_p_accel_cmd(data, accel_cmd_);
  set_p_steer_cmd(data, steer_cmd_);
  set_p_decel_cmd(data, decel_cmd_);
  set_p_direction_cmd(data, dir_cmd_);
}

void Accelcmd1e3::Reset() {
  // TODO(QiL) :you should check this manually
  accel_cmd_ = 0.0;
  steer_cmd_ = 0.0;
  decel_cmd_ = 0.0;
  dir_cmd_ = 1;
}

Accelcmd1e3* Accelcmd1e3::set_accel_cmd(double accel_cmd) {
  accel_cmd_ = accel_cmd;
  return this;
}

Accelcmd1e3* Accelcmd1e3::set_steer_cmd(double steer_cmd) {
  steer_cmd_ = steer_cmd;
  return this;
}

Accelcmd1e3* Accelcmd1e3::set_decel_cmd(double decel_cmd) {
  decel_cmd_ = decel_cmd;
  return this;
}

Accelcmd1e3* Accelcmd1e3::set_direction_cmd(uint32_t dir_cmd) {
  dir_cmd_ = dir_cmd;
  return this;
}

void Accelcmd1e3::set_p_accel_cmd(uint8_t* data, double accel_cmd) {
  if (accel_cmd > 100.) accel_cmd = 100.;
  if (accel_cmd < 0.) accel_cmd = 0.;
  int x = accel_cmd;
  uint8_t t = 0;

  t = x & 0xFF;
  Byte to_set0(data);
  to_set0.set_value(t, 0, 8);
}

void Accelcmd1e3::set_p_steer_cmd(uint8_t* data, double steer_cmd) {
  if (steer_cmd > 100.) steer_cmd = 100.;
  if (steer_cmd < -100.) steer_cmd = -100.;
  int x = steer_cmd;
  uint8_t t = 0;

  t = x & 0xFF;
  Byte to_set1(data + 1);
  to_set1.set_value(t, 0, 8);
}

void Accelcmd1e3::set_p_decel_cmd(uint8_t* data, double decel_cmd) {
  if (decel_cmd > 100.) decel_cmd = 100.;
  if (decel_cmd < 0.) decel_cmd = 0.;
  int x = decel_cmd;
  uint8_t t = 0;

  t = x & 0xFF;
  Byte to_set2(data + 2);
  to_set2.set_value(t, 0, 8);
}

void Accelcmd1e3::set_p_direction_cmd(uint8_t* data, uint32_t dir_cmd) {
  if (dir_cmd != 0x00 && dir_cmd != 0x01 && dir_cmd != 0x02)
    dir_cmd = 0x0;

  uint8_t t = 0;

  t = dir_cmd & 0xFF;
  Byte to_set(data + 3);
  to_set.set_value(1, 0, 1);
  to_set.set_value(t, 1, 2);
}

}  // namespace agribot
}  // namespace canbus
}  // namespace apollo
